// v20 08/08/14 effort to get all 4 templates/locations
// v30 08/08/14 combined with Homography RANSAC & camera read
// v32 09/09/14 copied frankly from v20 to test
// v33 09/09/14 from v32 develop to wc version

#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <iostream>
#include <stdio.h>

#include "MatWork.hpp"

using namespace std;
using namespace cv;

/// Global Variables
Mat img, img_org, img_new, img_dsp_org;
Mat templ; Mat result;
char* image_window = "Source Image";
char* result_window = "Result window";

int match_method;
int max_Trackbar = 5;

/// Tracker variables
int track_flg = 0;

/// Template matcher vars
Mat img_display;
Mat mask;
int result_cols;
int result_rows;
double minVal; double maxVal; Point minLoc; Point maxLoc;
Point matchLoc;

/// Function Headers
void MatchingMethod( int, void* );


/** @function main */
int main( int argc, char** argv )
{
	/// Load image and template
	img = imread( "Picture 46.jpg", 1 );
	img = imread( "Picture 48.jpg", 1 );
	//img = imread( "Picture 49.jpg", 1 );
	//img = imread( "Picture 50.jpg", 1 );
	img.copyTo(img_org);
	img.copyTo(img_new);
	img.copyTo(img_dsp_org);

	templ = imread( "Picture 47_templ.jpg", 1 );

	// Create camera
	VideoCapture cap(0); // open the default camera
	if(!cap.isOpened())  // check if we succeeded
	  return -1;


	/// Create windows
	namedWindow( image_window, CV_WINDOW_AUTOSIZE );
	//namedWindow( result_window, CV_WINDOW_AUTOSIZE );

	/// Create Trackbar
	//char* trackbar_label = "Method: \n 0: SQDIFF \n 1: SQDIFF NORMED \n 2: TM CCORR \n 3: TM CCORR NORMED \n 4: TM COEFF \n 5: TM COEFF NORMED";
	//createTrackbar( trackbar_label, image_window, &match_method, max_Trackbar, MatchingMethod );

	// START THE LOOP
	while (1)
	{
		// Get image from webcam
		cap >> img;

		img.copyTo(img_org);
		img.copyTo(img_dsp_org);

		// Perform template search for new dots
		if (track_flg)
		for (int i=0; i<4; i++)
		{
			MatchingMethod( 0, 0 );

			//printf("Location: (%d,%d)\n",matchLoc.x,matchLoc.y);
			////MATmtxdisp(mask);

			//// draw on the input image
			//rectangle( img_display, matchLoc, Point( matchLoc.x + templ.cols , matchLoc.y + templ.rows ), Scalar(255,255,0), 2, 8, 0 );
			//line( img_display, Point( matchLoc.x + templ.cols/2 , matchLoc.y + templ.rows/2 - 10) , Point( matchLoc.x + templ.cols/2 , matchLoc.y + templ.rows/2 + 10 ), Scalar(255,255,0), 2, 8, 0 );
			//line( img_display, Point( matchLoc.x + templ.cols/2 - 10 , matchLoc.y + templ.rows/2 ) , Point( matchLoc.x + templ.cols/2 + 10, matchLoc.y + templ.rows/2), Scalar(255,255,0), 2, 8, 0 );
			//img_org(Range(matchLoc.y, matchLoc.y + templ.rows), Range(matchLoc.x, matchLoc.x + templ.cols)) = Scalar(255,255,255);

			//// draw on the orginal image
			//// img_org.copyTo(img_new);
			//rectangle( img_display, matchLoc, Point( matchLoc.x + templ.cols , matchLoc.y + templ.rows ), Scalar(255,255,0), 2, 8, 0 );
			//line( img_display, Point( matchLoc.x + templ.cols/2 , matchLoc.y + templ.rows/2 - 10) , Point( matchLoc.x + templ.cols/2 , matchLoc.y + templ.rows/2 + 10 ), Scalar(255,255,0), 2, 8, 0 );
			//line( img_display, Point( matchLoc.x + templ.cols/2 - 10 , matchLoc.y + templ.rows/2 ) , Point( matchLoc.x + templ.cols/2 + 10, matchLoc.y + templ.rows/2), Scalar(255,255,0), 2, 8, 0 );

			//rectangle( result, matchLoc, Point( matchLoc.x + templ.cols , matchLoc.y + templ.rows ), Scalar::all(0), 2, 8, 0 );
		}

		//img_new.copyTo(img_org);

		// Call Homography matching
		

		// Generate new estimated points


		// Draw the output
		/// Show me what you got
		//printf("Location: (%d,%d)\n",matchLoc.x,matchLoc.y);
		//MATmtxdisp(mask);

		// draw on the input image
		/*rectangle( img_display, matchLoc, Point( matchLoc.x + templ.cols , matchLoc.y + templ.rows ), Scalar(255,255,0), 2, 8, 0 );
		line( img_display, Point( matchLoc.x + templ.cols/2 , matchLoc.y + templ.rows/2 - 10) , Point( matchLoc.x + templ.cols/2 , matchLoc.y + templ.rows/2 + 10 ), Scalar(255,255,0), 2, 8, 0 );
		line( img_display, Point( matchLoc.x + templ.cols/2 - 10 , matchLoc.y + templ.rows/2 ) , Point( matchLoc.x + templ.cols/2 + 10, matchLoc.y + templ.rows/2), Scalar(255,255,0), 2, 8, 0 );
		img(Range(matchLoc.y, matchLoc.y + templ.rows), Range(matchLoc.x, matchLoc.x + templ.cols)) = Scalar(255,255,255);*/

		// draw on the orginal image
		// img_org.copyTo(img_dsp_org);
		/*rectangle( img_dsp_org, matchLoc, Point( matchLoc.x + templ.cols , matchLoc.y + templ.rows ), Scalar(255,255,0), 2, 8, 0 );
		line( img_dsp_org, Point( matchLoc.x + templ.cols/2 , matchLoc.y + templ.rows/2 - 10) , Point( matchLoc.x + templ.cols/2 , matchLoc.y + templ.rows/2 + 10 ), Scalar(255,255,0), 2, 8, 0 );
		line( img_dsp_org, Point( matchLoc.x + templ.cols/2 - 10 , matchLoc.y + templ.rows/2 ) , Point( matchLoc.x + templ.cols/2 + 10, matchLoc.y + templ.rows/2), Scalar(255,255,0), 2, 8, 0 );*/

		//rectangle( result, matchLoc, Point( matchLoc.x + templ.cols , matchLoc.y + templ.rows ), Scalar::all(0), 2, 8, 0 );

		//imshow( image_window, img_display );
		//imshow( "edited image", img);
		//imshow( "original image with marker", img_dsp_org);
		//imshow( result_window, result );

		//imshow("WC image", img);
		//imshow("Tracked image", img_display);

		//waitKey(0);

		// ############ KEYBOARD PROCESSING
		char c = waitKey(1);

		if (c == 's')
		{
			track_flg = 1;
			img.copyTo(img_org);
		}

		if (c == 'b')
		{
			track_flg = 0;
		}

		if (c == 27)
			break;
	}

	return 0;
}

/**
 * @function MatchingMethod
 * @brief Trackbar callback
 */
void MatchingMethod( int, void* )
{
	/// Source image to display
	Mat img_display;
	img.copyTo( img_display );
	Mat mask;

	/// Create the result matrix
	int result_cols =  img.cols - templ.cols + 1;
	int result_rows = img.rows - templ.rows + 1;

	result.create( result_cols, result_rows, CV_32FC1 );

	/// Do the Matching and Normalize
	matchTemplate( img, templ, result, match_method );
	normalize( result, result, 0, 1, NORM_MINMAX, -1, Mat() );

	/// Localizing the best match with minMaxLoc
	double minVal; double maxVal; Point minLoc; Point maxLoc;
	Point matchLoc;

	minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, mask );

	/// For SQDIFF and SQDIFF_NORMED, the best matches are lower values. For all the other methods, the higher the better
	if( match_method  == CV_TM_SQDIFF || match_method == CV_TM_SQDIFF_NORMED )
	{ matchLoc = minLoc; }
	else
	{ matchLoc = maxLoc; }

	/// Show me what you got
	printf("Location: (%d,%d)\n",matchLoc.x,matchLoc.y);
	MATmtxdisp(mask);

	// draw on the input image
	rectangle( img_display, matchLoc, Point( matchLoc.x + templ.cols , matchLoc.y + templ.rows ), Scalar(255,255,0), 2, 8, 0 );
	line( img_display, Point( matchLoc.x + templ.cols/2 , matchLoc.y + templ.rows/2 - 10) , Point( matchLoc.x + templ.cols/2 , matchLoc.y + templ.rows/2 + 10 ), Scalar(255,255,0), 2, 8, 0 );
	line( img_display, Point( matchLoc.x + templ.cols/2 - 10 , matchLoc.y + templ.rows/2 ) , Point( matchLoc.x + templ.cols/2 + 10, matchLoc.y + templ.rows/2), Scalar(255,255,0), 2, 8, 0 );
	img(Range(matchLoc.y, matchLoc.y + templ.rows), Range(matchLoc.x, matchLoc.x + templ.cols)) = Scalar(255,255,255);

	// draw on the orginal image
	// img_org.copyTo(img_dsp_org);
	rectangle( img_dsp_org, matchLoc, Point( matchLoc.x + templ.cols , matchLoc.y + templ.rows ), Scalar(255,255,0), 2, 8, 0 );
	line( img_dsp_org, Point( matchLoc.x + templ.cols/2 , matchLoc.y + templ.rows/2 - 10) , Point( matchLoc.x + templ.cols/2 , matchLoc.y + templ.rows/2 + 10 ), Scalar(255,255,0), 2, 8, 0 );
	line( img_dsp_org, Point( matchLoc.x + templ.cols/2 - 10 , matchLoc.y + templ.rows/2 ) , Point( matchLoc.x + templ.cols/2 + 10, matchLoc.y + templ.rows/2), Scalar(255,255,0), 2, 8, 0 );

	//rectangle( result, matchLoc, Point( matchLoc.x + templ.cols , matchLoc.y + templ.rows ), Scalar::all(0), 2, 8, 0 );

	imshow( image_window, img_display );
	imshow( "edited image", img);
	imshow( "original image with marker", img_dsp_org);
	//imshow( result_window, result );

	return;
}